# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

import math
from isaaclab_assets.robots.cartpole import CARTPOLE_CFG

from isaaclab.assets import ArticulationCfg
from isaaclab.envs import DirectRLEnvCfg
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sim import SimulationCfg
from isaaclab.utils import configclass





# -----------------------------------------fanziqi
import isaaclab.sim as sim_utils
from isaaclab.actuators import DCMotorCfg
# from isaaclab.assets.articulation import ArticulationCfg
YMBOT_E_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        # usd_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Robots/Unitree/A1/a1.usd",
        usd_path="/home/ps/sh_robot/hofa_rl/hofa_rl_isaaclab/source/hofa_rl_isaaclab/hofa_rl_isaaclab/assets/ymbot_e/ymbot_e.usd",
        activate_contact_sensors=True,
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False,
            retain_accelerations=False,
            linear_damping=0.0,
            angular_damping=0.0,
            max_linear_velocity=1000.0,
            max_angular_velocity=1000.0,
            max_depenetration_velocity=1.0,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=False, 
            solver_position_iteration_count=4, 
            solver_velocity_iteration_count=0,
            fix_root_link=True,
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=(0.0, 0.0, 1.8),
        joint_pos={
            # ".*L_hip_joint": 0.0,
            # ".*R_hip_joint": -0.0,
            # "F.*_thigh_joint": 0.8,
            # "R.*_thigh_joint": 0.8,
            # ".*_calf_joint": -1.5,
            ".*": 0.6 # !!!
        },
        joint_vel={".*": 0.0},
    ),
    soft_joint_pos_limit_factor=0.9,
    actuators={
        "legs": DCMotorCfg(
            # joint_names_expr=[".*_Joint\\d*"],
            joint_names_expr=["Neck_Joint2", "Torso_Joint2"],
            effort_limit=33.5,
            saturation_effort=33.5,
            velocity_limit=21.0,
            stiffness=20.0,
            damping=0.5,
            friction=0.0,
        ),
    },
)
"""Configuration of YMBOT_E using DC motor."""




@configclass
class YmbotEJointTrackEnvCfg(DirectRLEnvCfg):
    # env
    decimation = 10
    episode_length_s = 15.0 # 秒 
    # max_episode_length = math.ceil(self.max_episode_length_s / (self.cfg.sim.dt * self.cfg.decimation))
    
    # - spaces definition
    action_space = 2
    observation_space = 4
    state_space = 0

    # simulation
    sim: SimulationCfg = SimulationCfg(dt=1 / 1000, render_interval=decimation)

    # robot(s)
    robot_cfg: ArticulationCfg = YMBOT_E_CFG.replace(prim_path="/World/envs/env_.*/Robot")

    # scene
    scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True)

    # custom parameters/scales
    # - action scale
    action_scale = 1.0  # [0, 1.0]
    # - reward scales
    rew_scale_alive = 1.0
    rew_scale_terminated = -2.0
    rew_scale_joint_pos = -1.0
    rew_scale_joint_vel = -0.01
    # - reset states/conditions
    initial_pole_angle_range = [-1.0, 1.0]  # pole angle sample range on reset [rad]
    max_joint_pos = 60 * (math.pi / 180)   # reset if any joint exceeds this radian [rad]